
// import haxe.ds.Array;
// import oimo.common.*;
// import oimo.dynamics.*;
// import oimo.dynamics.constraint.*;
// import oimo.dynamics.constraint.info.joint.*;
// import oimo.dynamics.constraint.solver.*;
// import oimo.dynamics.constraint.solver.direct.*;
// import oimo.dynamics.constraint.solver.pgs.*;
// import oimo.dynamics.rigidbody.*;
// import oimo.m.Mat3;
// import oimo.m.Quat;
// import oimo.m.Vec3;
// import oimo.m.M;

import { RigidBody } from "../../rigidbody/RigidBody";
import { Vec3 } from "../../../common/Vec3";
import { JointImpulse } from "./JointImpulse";
import { ConstraintSolver } from "../ConstraintSolver";
import { Setting } from "../../../common/Setting";
import { ConstraintSolverType } from "../solver/ConstraintSolverType";
import { DirectJointConstraintSolver } from "../solver/direct/DirectJointConstraintSolver";
import { PgsJointConstraintSolver } from "../solver/pgs/PgsJointConstraintSolver";
import { M } from "../../../common/M";
import { Quat } from "../../../common/Quat";
import { Mat3 } from "../../../common/Mat3";
import { Transform } from "../../../common/Transform";
import { JointSolverInfoRow } from "../info/joint/JointSolverInfoRow";
import { TimeStep } from "../../TimeStep";
import { MathUtil } from "../../../common/MathUtil";
import { PositionCorrectionAlgorithm } from "../PositionCorrectionAlgorithm";
import { JointSolverInfo } from "../info/joint/JointSolverInfo";
import { JointLink } from "./JointLink";
import { JointConfig } from "./JointConfig";
import { World } from "../../World";
import { TranslationalLimitMotor } from "./TranslationalLimitMotor";
import { JointMacro } from "./JointMacro";
import { SpringDamper } from "./SpringDamper";
import { RotationalLimitMotor } from "./RotationalLimitMotor";

/**
 * The base class of joints. Joints are used to connect two rigid bodies
 * in various ways. See `JointType` for all types of joints.
 */
export class Joint {
	_b1: RigidBody;
	_b2: RigidBody;

	_link1: JointLink;
	_link2: JointLink;
	_positionCorrectionAlgorithm: number;

	_allowCollision: boolean;

	_prev: Joint;
	_next: Joint;
	_world: World;

	_localAnchor1: Vec3;
	_localAnchor2: Vec3;
	_relativeAnchor1: Vec3;
	_relativeAnchor2: Vec3;
	_anchor1: Vec3;
	_anchor2: Vec3;

	_localBasisX1: Vec3;
	_localBasisY1: Vec3;
	_localBasisZ1: Vec3;
	_localBasisX2: Vec3;
	_localBasisY2: Vec3;
	_localBasisZ2: Vec3;

	_basisX1: Vec3;
	_basisY1: Vec3;
	_basisZ1: Vec3;
	_basisX2: Vec3;
	_basisY2: Vec3;
	_basisZ2: Vec3;

	_impulses: Array<JointImpulse>;

	// computed in constraint solver
	_appliedForce: Vec3;
	_appliedTorque: Vec3;

	_breakForce: number;
	_breakTorque: number;

	_type: number;

	_solver: ConstraintSolver;

	/**
	 * Extra field that users can use for their own purposes.
	 */
	userData: any;


	constructor(config: JointConfig, type: number) {
		this._link1 = new JointLink(this);
		this._link2 = new JointLink(this);
		this._positionCorrectionAlgorithm = Setting.defaultJointPositionCorrectionAlgorithm;
		this._type = type;
		this._world = null;
		this._b1 = config.rigidBody1;
		this._b2 = config.rigidBody2;
		this._allowCollision = config.allowCollision;
		this._breakForce = config.breakForce;
		this._breakTorque = config.breakTorque;

		switch (config.solverType) {
			case ConstraintSolverType._DIRECT:
				this._solver = new DirectJointConstraintSolver(this);
				break;
			case ConstraintSolverType._ITERATIVE:
				this._solver = new PgsJointConstraintSolver(this);
				break;
		}

		this._localAnchor1 = config.localAnchor1.clone();
		this._localAnchor2 = config.localAnchor2.clone();

		this._relativeAnchor1 = new Vec3()
		this._relativeAnchor2 = new Vec3();
		this._anchor1 = new Vec3();
		this._anchor2 = new Vec3();

		this._localBasisX1 = new Vec3();
		this._localBasisY1 = new Vec3();
		this._localBasisZ1 = new Vec3();
		this._localBasisX2 = new Vec3();
		this._localBasisY2 = new Vec3();
		this._localBasisZ2 = new Vec3();

		this._impulses = new Array<JointImpulse>(Setting.maxJacobianRows);
		for (let i = 0; i < Setting.maxJacobianRows; ++i) {
			this._impulses[i] = new JointImpulse();
		}
	}

	// --- ---
	public buildLocalBasesFromX(): void {
		// validate X
		if (M.vec3_dot(this._localBasisX1, this._localBasisX1) == 0) {
			this._localBasisX1.init(1, 0, 0);
		} else {
			this._localBasisX1.normalize();
		}
		if (M.vec3_dot(this._localBasisX2, this._localBasisX2) == 0) {
			this._localBasisX2.init(1, 0, 0);
		} else {
			this._localBasisX2.normalize();
		}

		var slerpQ: Quat = new Quat();
		var slerpM: Mat3 = new Mat3();
		M.quat_arc(slerpQ, this._localBasisX1, this._localBasisX2);
		M.mat3_fromQuat(slerpM, slerpQ);

		M.vec3_perp(this._localBasisY1, this._localBasisX1);
		M.vec3_cross(this._localBasisZ1, this._localBasisX1, this._localBasisY1);

		M.vec3_mulMat3(this._localBasisX2, this._localBasisX1, slerpM);
		M.vec3_mulMat3(this._localBasisY2, this._localBasisY1, slerpM);
		M.vec3_mulMat3(this._localBasisZ2, this._localBasisZ1, slerpM);
	}


	private buildLocalBasesFromXY(): void {
		// validate X
		if (M.vec3_dot(this._localBasisX1, this._localBasisX1) == 0) {
			M.vec3_set(this._localBasisX1, 1, 0, 0);
		} else {
			M.vec3_normalize(this._localBasisX1, this._localBasisX1);
		}

		if (M.vec3_dot(this._localBasisX2, this._localBasisX2) == 0) {
			M.vec3_set(this._localBasisX2, 1, 0, 0);
		} else {
			M.vec3_normalize(this._localBasisX2, this._localBasisX2);
		}

		// build Z and recompute Y
		M.vec3_cross(this._localBasisZ1, this._localBasisX1, this._localBasisY1);
		M.vec3_cross(this._localBasisZ2, this._localBasisX2, this._localBasisY2);

		if (M.vec3_dot(this._localBasisZ1, this._localBasisZ1) == 0) {
			// invalid Y, set Y perpendicular to X
			M.vec3_perp(this._localBasisY1, this._localBasisX1);
			// Z = cross(X, Y)
			M.vec3_cross(this._localBasisZ1, this._localBasisX1, this._localBasisY1);
		} else {
			// normalize Z
			M.vec3_normalize(this._localBasisZ1, this._localBasisZ1);
			// Y = cross(Z, X)
			M.vec3_cross(this._localBasisY1, this._localBasisZ1, this._localBasisX1);
		}

		if (M.vec3_dot(this._localBasisZ2, this._localBasisZ2) == 0) {
			// invalid Y, set Y perpendicular to X
			M.vec3_perp(this._localBasisY2, this._localBasisX2);
			// Z = cross(X, Y)
			M.vec3_cross(this._localBasisZ2, this._localBasisX2, this._localBasisY2);
		} else {
			// normalize Z
			M.vec3_normalize(this._localBasisZ2, this._localBasisZ2);
			// Y = cross(Z, X)
			M.vec3_cross(this._localBasisY2, this._localBasisZ2, this._localBasisX2);
		}
	}

	public buildLocalBasesFromX1Z2(): void {
		// validate X1 and Z2
		if (M.vec3_dot(this._localBasisX1, this._localBasisX1) == 0) {
			M.vec3_set(this._localBasisX1, 1, 0, 0);
		} else {
			M.vec3_normalize(this._localBasisX1, this._localBasisX1);
		}

		if (M.vec3_dot(this._localBasisZ2, this._localBasisZ2) == 0) {
			M.vec3_set(this._localBasisZ2, 0, 0, 1);
		} else {
			M.vec3_normalize(this._localBasisZ2, this._localBasisZ2);
		}

		var tf1: Transform = this._b1._transform;
		var tf2: Transform = this._b2._transform;

		// compute world bases
		var worldX1: Vec3 = new Vec3();
		var worldZ1: Vec3 = new Vec3();
		var worldY: Vec3 = new Vec3();
		var worldX2: Vec3 = new Vec3();
		var worldZ2: Vec3 = new Vec3();
		M.vec3_mulMat3(worldX1, this._localBasisX1, tf1._rotation);
		M.vec3_mulMat3(worldZ2, this._localBasisZ2, tf2._rotation);
		M.vec3_cross(worldY, worldZ2, worldX1);

		if (M.vec3_dot(worldY, worldY) == 0) {
			M.vec3_perp(worldY, worldX1);
		}

		M.vec3_cross(worldZ1, worldX1, worldY);
		M.vec3_cross(worldX2, worldY, worldZ2);

		// return to local
		M.vec3_mulMat3Transposed(this._localBasisX1, worldX1, tf1._rotation);
		M.vec3_mulMat3Transposed(this._localBasisY1, worldY, tf1._rotation);
		M.vec3_mulMat3Transposed(this._localBasisZ1, worldZ1, tf1._rotation);
		M.vec3_mulMat3Transposed(this._localBasisX2, worldX2, tf2._rotation);
		M.vec3_mulMat3Transposed(this._localBasisY2, worldY, tf2._rotation);
		M.vec3_mulMat3Transposed(this._localBasisZ2, worldZ2, tf2._rotation);
	}


	public buildLocalBasesFromXY1X2(): void {
		// validate X1
		if (M.vec3_dot(this._localBasisX1, this._localBasisX1) == 0) {
			M.vec3_set(this._localBasisX1, 1, 0, 0);
		} else {
			M.vec3_normalize(this._localBasisX1, this._localBasisX1);
		}

		// build Z1 and recompute Y1
		M.vec3_cross(this._localBasisZ1, this._localBasisX1, this._localBasisY1);

		if (M.vec3_dot(this._localBasisZ1, this._localBasisZ1) == 0) {
			// invalid Y1, set Y1 perpendicular to X1
			M.vec3_perp(this._localBasisY1, this._localBasisX1);
			// Z1 = cross(X1, Y1)
			M.vec3_cross(this._localBasisZ1, this._localBasisX1, this._localBasisY1);
		} else {
			// normalize Z1
			M.vec3_normalize(this._localBasisZ1, this._localBasisZ1);
			// Y1 = cross(Z1, X1)
			M.vec3_cross(this._localBasisY1, this._localBasisZ1, this._localBasisX1);
		}

		// rotate (X1, Y1, Z1) to (X2, Y2, Z2) by arc(X1, X2)
		var slerpQ: Quat = new Quat();
		var slerpM: Mat3 = new Mat3();
		M.quat_arc(slerpQ, this._localBasisX1, this._localBasisX2);
		M.mat3_fromQuat(slerpM, slerpQ);

		M.vec3_mulMat3(this._localBasisX2, this._localBasisX1, slerpM);
		M.vec3_mulMat3(this._localBasisY2, this._localBasisY1, slerpM);
		M.vec3_mulMat3(this._localBasisZ2, this._localBasisZ1, slerpM);
	}


	public setSolverInfoRowLinear(row: JointSolverInfoRow, diff: number, lm: TranslationalLimitMotor,
		mass: number, sd: SpringDamper, timeStep: TimeStep, isPositionPart: boolean): void {
		var cfmFactor: number;
		var erp: number;
		var slop: number = Setting.linearSlop;

		if (isPositionPart) {
			cfmFactor = 0;
			erp = 1;
		} else {
			if (sd.frequency > 0) {
				// the constraint is softened
				slop = 0;
				let result = JointMacro.computeSoftConstraintParameters(sd.frequency, sd.dampingRatio, timeStep.dt, sd.useSymplecticEuler);
				cfmFactor = result[0];
				erp = result[1];
			} else {
				// the constraint is rigid
				cfmFactor = 0;
				erp = this.getErp(timeStep, false);
			}

			// set motor parameters if enabled
			if (lm.motorForce > 0) {
				row.motor(lm.motorSpeed, lm.motorForce * timeStep.dt);
			} else {
				row.motor(0, 0);
			}
		}

		var lower: number = lm.lowerLimit;
		var upper: number = lm.upperLimit;

		var minImp: number;
		var maxImp: number;
		var error: number;
		if (lower > upper) {
			// inactive
			minImp = 0;
			maxImp = 0;
			error = 0;
		} else if (lower == upper) {
			// locked
			minImp = MathUtil.NEGATIVE_INFINITY;
			maxImp = MathUtil.POSITIVE_INFINITY;
			error = diff - lower;
		} else if (diff < lower) {
			// at lower limit
			minImp = MathUtil.NEGATIVE_INFINITY;
			maxImp = 0;
			error = diff - lower + slop;
			if (error > 0) {
				error = 0;
			}
		} else if (diff > upper) {
			// at upper limit
			minImp = 0;
			maxImp = MathUtil.POSITIVE_INFINITY;
			error = diff - upper - slop;
			if (error < 0) {
				error = 0;
			}
		} else {
			// inactive
			minImp = 0;
			maxImp = 0;
			error = 0;
		}

		// inverse motor mass
		var invMass: number = mass == 0 ? 0 : 1 / mass;

		row.minImpulse = minImp;
		row.maxImpulse = maxImp;
		row.cfm = cfmFactor * invMass;
		row.rhs = error * erp;
	}


	public setSolverInfoRowAngular(row: JointSolverInfoRow, diff: number, lm: RotationalLimitMotor,
		mass: number, sd: SpringDamper, timeStep: TimeStep, isPositionPart: boolean): void {
		var cfmFactor: number;
		var erp: number;
		var slop: number = Setting.angularSlop;

		if (isPositionPart) {
			cfmFactor = 0;
			erp = 1;
		} else {
			if (sd.frequency > 0) {
				// the constraint is softened
				slop = 0;
				let result = JointMacro.computeSoftConstraintParameters(sd.frequency, sd.dampingRatio, timeStep.dt, sd.useSymplecticEuler);
				cfmFactor = result[0];
				erp = result[1];
			} else {
				// the constraint is rigid
				cfmFactor = 0;
				erp = this.getErp(timeStep, false);
			}

			// set motor parameters if enabled
			if (lm.motorTorque > 0) {
				row.motor(lm.motorSpeed, lm.motorTorque * timeStep.dt);
			} else {
				row.motor(0, 0);
			}
		}

		var lower: number = lm.lowerLimit;
		var upper: number = lm.upperLimit;

		// adjust theta (in [-pi, pi] => in [mid - pi, mid + pi])
		var mid: number = (lower + upper) * 0.5;
		diff -= mid;
		diff = ((diff + MathUtil.PI) % MathUtil.TWO_PI + MathUtil.TWO_PI) % MathUtil.TWO_PI - MathUtil.PI;
		diff += mid;

		var minImp: number;
		var maxImp: number;
		var error: number;
		if (lower > upper) {
			// inactive
			minImp = 0;
			maxImp = 0;
			error = 0;
		} else if (lower == upper) {
			// locked
			minImp = MathUtil.NEGATIVE_INFINITY;
			maxImp = MathUtil.POSITIVE_INFINITY;
			error = diff - lower;
		} else if (diff < lower) {
			// at lower limit
			minImp = MathUtil.NEGATIVE_INFINITY;
			maxImp = 0;
			error = diff - lower + slop;
			if (error > 0) {
				error = 0;
			}
		} else if (diff > upper) {
			// at upper limit
			minImp = 0;
			maxImp = MathUtil.POSITIVE_INFINITY;
			error = diff - upper - slop;
			if (error < 0) {
				error = 0;
			}
		} else {
			// inactive
			minImp = 0;
			maxImp = 0;
			error = 0;
		}

		// inverse motor mass
		var invMass: number = mass == 0 ? 0 : 1 / mass;

		row.minImpulse = minImp;
		row.maxImpulse = maxImp;
		row.cfm = cfmFactor * invMass;
		row.rhs = error * erp;
	}


	public getErp(timeStep: TimeStep, isPositionPart: boolean): number {
		if (isPositionPart) {
			return 1;
		} else {
			if (this._positionCorrectionAlgorithm == PositionCorrectionAlgorithm.BAUMGARTE) {
				return timeStep.invDt * Setting.velocityBaumgarte;
			} else {
				return 0;
			}
		}
	}


	public computeEffectiveInertiaMoment(axis: Vec3): number {
		var ia1: Vec3 = new Vec3();
		var ia2: Vec3 = new Vec3();
		M.vec3_mulMat3(ia1, axis, this._b1._invInertia);
		M.vec3_mulMat3(ia2, axis, this._b2._invInertia);
		var invI1: number = M.vec3_dot(ia1, axis);
		var invI2: number = M.vec3_dot(ia2, axis);
		if (invI1 > 0) {
			var rsq: number = M.vec3_dot(this._relativeAnchor1, this._relativeAnchor1);
			var dot: number = M.vec3_dot(axis, this._relativeAnchor1);
			var projsq: number = rsq - dot * dot;
			if (projsq > 0) {
				if (this._b1._invMass > 0) {
					invI1 = 1 / (1 / invI1 + this._b1._mass * projsq);
				} else {
					invI1 = 0;
				}
			}
		}
		if (invI2 > 0) {
			var rsq: number = M.vec3_dot(this._relativeAnchor2, this._relativeAnchor2);
			var dot: number = M.vec3_dot(axis, this._relativeAnchor2);
			var projsq: number = rsq - dot * dot;
			if (projsq > 0) {
				if (this._b2._invMass > 0) {
					invI2 = 1 / (1 / invI2 + this._b2._mass * projsq);
				} else {
					invI2 = 0;
				}
			}
		}
		return invI1 + invI2 == 0 ? 0 : 1 / (invI1 + invI2);
	}


	private computeEffectiveInertiaMoment2(axis1: Vec3, axis2: Vec3): number {
		var ia1: Vec3 = new Vec3();
		var ia2: Vec3 = new Vec3();
		M.vec3_mulMat3(ia1, axis1, this._b1._invInertia);
		M.vec3_mulMat3(ia2, axis2, this._b2._invInertia);
		var invI1: number = M.vec3_dot(ia1, axis1);
		var invI2: number = M.vec3_dot(ia2, axis2);
		if (invI1 > 0) {
			var rsq: number = M.vec3_dot(this._relativeAnchor1, this._relativeAnchor1);
			var dot: number = M.vec3_dot(axis1, this._relativeAnchor1);
			var projsq: number = rsq * rsq - dot * dot;
			if (projsq > 0) {
				if (this._b1._invMass > 0) {
					invI1 = 1 / (1 / invI1 + this._b1._mass * projsq);
				} else {
					invI1 = 0;
				}
			}
		}
		if (invI2 > 0) {
			var rsq: number = M.vec3_dot(this._relativeAnchor2, this._relativeAnchor2);
			var dot: number = M.vec3_dot(axis2, this._relativeAnchor2);
			var projsq: number = rsq * rsq - dot * dot;
			if (projsq > 0) {
				if (this._b2._invMass > 0) {
					invI2 = 1 / (1 / invI2 + this._b2._mass * projsq);
				} else {
					invI2 = 0;
				}
			}
		}
		return invI1 + invI2 == 0 ? 0 : 1 / (invI1 + invI2);
	}

	// --- internal ---


	public _getWarmStartingFactor(): number {
		switch (this._positionCorrectionAlgorithm) {
			case PositionCorrectionAlgorithm.BAUMGARTE:
				return Setting.jointWarmStartingFactorForBaungarte;
			default:
				return Setting.jointWarmStartingFactor;
		}
	}

	// !! don't forget to call this from constraint solver !!
	public _syncAnchors(): void {
		var tf1: Transform;
		var tf2: Transform;
		tf1 = this._b1._transform;
		tf2 = this._b2._transform;

		// anchors
		M.vec3_mulMat3(this._relativeAnchor1, this._localAnchor1, tf1._rotation);
		M.vec3_mulMat3(this._relativeAnchor2, this._localAnchor2, tf2._rotation);
		M.vec3_add(this._anchor1, this._relativeAnchor1, tf1._position);
		M.vec3_add(this._anchor2, this._relativeAnchor2, tf2._position);

		// bases
		M.vec3_mulMat3(this._basisX1, this._localBasisX1, tf1._rotation);
		M.vec3_mulMat3(this._basisY1, this._localBasisY1, tf1._rotation);
		M.vec3_mulMat3(this._basisZ1, this._localBasisZ1, tf1._rotation);
		M.vec3_mulMat3(this._basisX2, this._localBasisX2, tf2._rotation);
		M.vec3_mulMat3(this._basisY2, this._localBasisY2, tf2._rotation);
		M.vec3_mulMat3(this._basisZ2, this._localBasisZ2, tf2._rotation);
	}

	public _getVelocitySolverInfo(timeStep: TimeStep, info: JointSolverInfo): void {
		info.b1 = this._b1;
		info.b2 = this._b2;
		info.numRows = 0;
	}

	public _getPositionSolverInfo(info: JointSolverInfo): void {
		info.b1 = this._b1;
		info.b2 = this._b2;
		info.numRows = 0;
	}

	public _checkDestruction(): void {
		var forceSq: number = M.vec3_dot(this._appliedForce, this._appliedForce);
		var torqueSq: number = M.vec3_dot(this._appliedTorque, this._appliedTorque);

		if (this._breakForce > 0 && forceSq > this._breakForce * this._breakForce) {
			this._world.removeJoint(this);
			return;
		}
		if (this._breakTorque > 0 && torqueSq > this._breakTorque * this._breakTorque) {
			this._world.removeJoint(this);
			return;
		}
	}


	public _attachLinks(): void {
		this._link1._other = this._b2;
		this._link2._other = this._b1;

		if (this._b1._jointLinkList == null) {
			this._b1._jointLinkList = this._link1;
			this._b1._jointLinkListLast = this._link1;
		} else {
			this._b1._jointLinkListLast._next = this._link1;
			this._link1._prev = this._b1._jointLinkListLast;
			this._b1._jointLinkListLast = this._link1;
		}
		// M.list_push(this._b1._jointLinkList, this._b1._jointLinkListLast, this._prev, this._next, this._link1);

		if (this._b2._jointLinkList == null) {
			this._b2._jointLinkList = this._link2;
			this._b2._jointLinkListLast = this._link2;
		} else {
			this._b2._jointLinkListLast._next = this._link2;
			this._link2._prev = this._b2._jointLinkListLast;
			this._b2._jointLinkListLast = this._link2;
		}
		// M.list_push(this._b2._jointLinkList, this._b2._jointLinkListLast, this._prev, this._next, this._link2);
		this._b1._numJointLinks++;
		this._b2._numJointLinks++;
		this._b1.wakeUp();
		this._b2.wakeUp();
	}


	public _detachLinks(): void {
		let prev = this._link1._prev;
		let next = this._link1._next;
		if (prev != null) {
			prev._next = next;
		}
		if (next != null) {
			next._prev = prev;
		}
		if (this._link1 == this._b1._jointLinkList) {
			this._b1._jointLinkList = next;
		}
		if (this._link1 == this._b1._jointLinkListLast) {
			this._b1._jointLinkListLast = prev;
		}
		this._link1._next = null;
		this._link1._prev = null;
		// M.list_remove(this._b1._jointLinkList, this._b1._jointLinkListLast, this._prev, this._next, this._link1);

		prev = this._link2._prev;
		next = this._link2._next;
		if (prev != null) {
			prev._next = next;
		}
		if (next != null) {
			next._prev = prev;
		}
		if (this._link2 == this._b2._jointLinkList) {
			this._b2._jointLinkList = next;
		}
		if (this._link2 == this._b2._jointLinkListLast) {
			this._b2._jointLinkListLast = prev;
		}
		this._link2._next = null;
		this._link2._prev = null;
		// M.list_remove(this._b2._jointLinkList, this._b2._jointLinkListLast, this._prev, this._next, this._link2);
		this._link1._other = null;
		this._link2._other = null;
		this._b1._numJointLinks--;
		this._b2._numJointLinks--;
		this._b1.wakeUp();
		this._b2.wakeUp();
	}

	// --- public ---

	/**
	 * Returns the first rigid body.
	 */
	public getRigidBody1(): RigidBody {
		return this._b1;
	}

	/**
	 * Returns the second rigid body.
	 */
	public getRigidBody2(): RigidBody {
		return this._b2;
	}

	/**
	 * Returns the type of the joint.
	 *
	 * See `JointType` for details.
	 */
	public getType(): number {
		return this._type;
	}

	/**
	 * Returns the first rigid body's anchor point in world coordinates.
	 */
	public getAnchor1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._anchor1);
		return v;
	}

	/**
	 * Returns the second rigid body's anchor point in world coordinates.
	 */
	public getAnchor2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._anchor2);
		return v;
	}

	/**
	 * Sets `anchor` to the first rigid body's anchor point in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAnchor1To(anchor: Vec3): void {
		M.vec3_toVec3(anchor, this._anchor1);
	}

	/**
	 * Sets `anchor` to the second rigid body's anchor point in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAnchor2To(anchor: Vec3): void {
		M.vec3_toVec3(anchor, this._anchor2);
	}

	/**
	 * Returns the first rigid body's local anchor point in world coordinates.
	 */
	public getLocalAnchor1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localAnchor1);
		return v;
	}

	/**
	 * Returns the second rigid body's local anchor point in world coordinates.
	 */
	public getLocalAnchor2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localAnchor2);
		return v;
	}

	/**
	 * Sets `localAnchor` to the first rigid body's anchor point in local coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAnchor1To(localAnchor: Vec3): void {
		M.vec3_toVec3(localAnchor, this._localAnchor1);
	}

	/**
	 * Sets `localAnchor` to the second rigid body's anchor point in local coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAnchor2To(localAnchor: Vec3): void {
		M.vec3_toVec3(localAnchor, this._localAnchor2);
	}

	/**
	 * Returns the basis of the joint for the first rigid body in world coordinates.
	 */
	public getBasis1(): Mat3 {
		var m: Mat3 = new Mat3();
		var b: Mat3 = new Mat3();
		M.mat3_fromCols(b, this._basisX1, this._basisY1, this._basisZ1);
		M.mat3_toMat3(m, b);
		return m;
	}

	/**
	 * Returns the basis of the joint for the second rigid body in world coordinates.
	 */
	public getBasis2(): Mat3 {
		var m: Mat3 = new Mat3();
		var b: Mat3 = new Mat3();
		M.mat3_fromCols(b, this._basisX2, this._basisY2, this._basisZ2);
		M.mat3_toMat3(m, b);
		return m;
	}

	/**
	 * Sets `basis` to the basis of the joint for the first rigid body in world coordinates.
	 *
	 * This does not create a new instance of `Mat3`.
	 */
	public getBasis1To(basis: Mat3): void {
		M.mat3_fromCols(basis, this._basisX1, this._basisY1, this._basisZ1);
	}

	/**
	 * Sets `basis` to the basis of the joint for the second rigid body in world coordinates.
	 *
	 * This does not create a new instance of `Mat3`.
	 */
	public getBasis2To(basis: Mat3): void {
		M.mat3_fromCols(basis, this._basisX2, this._basisY2, this._basisZ2);
	}

	/**
	 * Returns whether to allow the connected rigid bodies to collide each other.
	 */
	public getAllowCollision(): boolean {
		return this._allowCollision;
	}

	/**
	 * Sets whether to allow the connected rigid bodies to collide each other.
	 */
	public setAllowCollision(allowCollision: boolean): void {
		this._allowCollision = allowCollision;
	}

	/**
	 * Returns the magnitude of the constraint force at which the joint will be destroyed.
	 *
	 * Returns `0` if the joint is unbreakable.
	 */
	public getBreakForce(): number {
		return this._breakForce;
	}

	/**
	 * Sets the magnitude of the constraint force at which the joint will be destroyed.
	 *
	 * Set `0` for unbreakable joints.
	 */
	public setBreakForce(breakForce: number): void {
		this._breakForce = breakForce;
	}

	/**
	 * Returns the magnitude of the constraint torque at which the joint will be destroyed.
	 *
	 * Returns `0` if the joint is unbreakable.
	 */
	public getBreakTorque(): number {
		return this._breakTorque;
	}

	/**
	 * Sets the magnitude of the constraint force at which the joint will be destroyed.
	 *
	 * Set `0` for unbreakable joints.
	 */
	public setBreakTorque(breakTorque: number): void {
		this._breakTorque = breakTorque;
	}

	/**
	 * Returns the type of the position correction algorithm for the joint.
	 *
	 * See `PositionCorrectionAlgorithm` for details.
	 */
	public getPositionCorrectionAlgorithm(): number {
		return this._positionCorrectionAlgorithm;
	}

	/**
	 * Sets the type of the position correction algorithm to `positionCorrectionAlgorithm` for the joint.
	 *
	 * See `PositionCorrectionAlgorithm` for details.
	 */
	public setPositionCorrectionAlgorithm(positionCorrectionAlgorithm: number): void {
		switch (positionCorrectionAlgorithm) {
			case
				PositionCorrectionAlgorithm._BAUMGARTE,
				PositionCorrectionAlgorithm._SPLIT_IMPULSE,
				PositionCorrectionAlgorithm._NGS
				:
				break;
			default:
				throw M.error("invalid position correction algorithm id: " + positionCorrectionAlgorithm);
		}
		this._positionCorrectionAlgorithm = positionCorrectionAlgorithm;
	}

	/**
	 * Returns the force applied to the first rigid body at the last time step.
	 */
	public getAppliedForce(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._appliedForce);
		return v;
	}

	/**
	 * Sets `appliedForce` to the force applied to the first rigid body at the last time step.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAppliedForceTo(appliedForce: Vec3): void {
		M.vec3_toVec3(appliedForce, this._appliedForce);
	}

	/**
	 * Returns the torque applied to the first rigid body at the last time step.
	 */
	public getAppliedTorque(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._appliedTorque);
		return v;
	}

	/**
	 * Sets `appliedTorque` to the torque applied to the first rigid body at the last time step.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAppliedTorqueTo(appliedTorque: Vec3): void {
		M.vec3_toVec3(appliedTorque, this._appliedTorque);
	}

	/**
	 * Returns the previous joint in the world.
	 *
	 * If the previous one does not exist, `null` will be returned.
	 */
	public getPrev(): Joint {
		return this._prev;
	}

	/**
	 * Returns the next joint in the world.
	 *
	 * If the next one does not exist, `null` will be returned.
	 */
	public getNext(): Joint {
		return this._next;
	}
}
